// --------------------------------------------------------------------------------------------------------------------
// <copyright file="PicknPlaceAgentTest.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Hardware.PicknPlace
{
    using System;
    using System.Collections.Generic;
    using System.Diagnostics;
    using System.Drawing;
    using System.IO;
    using System.Runtime.Serialization;
    using System.Threading;
    using Microsoft.Robotics.Hardware.KukaLBR;
    using Microsoft.Robotics.Hardware.Runtime;
    using Microsoft.Robotics.Hardware.Runtime.Robotiq;
    using Microsoft.Robotics.Manipulation.PicknPlace;
    using Microsoft.Robotics.Manipulation.Runtime;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.PhysicalObjects;
    using Microsoft.Robotics.Runtime;
    using Microsoft.VisualStudio.TestTools.UnitTesting;
    using Test.Robotics.Hardware.KukaLBR.KukaLBR4Tests;
    using Test.Robotics.Runtime;

    /// <summary>
    /// Pick and Place agent tests
    /// </summary>
    [TestClass]
    public class PicknPlaceAgentTest
    {
        /// <summary>
        /// Test pick and place at the agent level using mock arm and gripper
        /// Runs 2 pick and place tasks
        /// </summary>
        [Priority(1)]
        [TestMethod]
        [TestCategory("Unit")]
        public void ManipulationPicknPlaceKukaRobotiqAgentTest()
        {
            List<Tuple<Pose?, Pose?, Pose?, Pose?, PicknPlaceStatusEnum, PicknPlaceStatusEnum, PicknPlaceStatusEnum, Tuple<PicknPlaceStatusEnum>>> tasks =
                    new List<Tuple<Pose?, Pose?, Pose?, Pose?, PicknPlaceStatusEnum, PicknPlaceStatusEnum, PicknPlaceStatusEnum, Tuple<PicknPlaceStatusEnum>>>();

            tasks.Add(Tuple.Create(
                new Pose(new Vector3(-0.351, -0.7614, 0.00)) as Pose?,
                new Pose(new Vector3(-0.650, -0.000, 0.3)) as Pose?,
                new Pose(new Vector3(-0.42, -0.5, 0.000)) as Pose?,
                new Pose(new Vector3(-0.600, -0.000, 0.3)) as Pose?,
                PicknPlaceStatusEnum.PickInProgress,
                PicknPlaceStatusEnum.PickComplete,
                PicknPlaceStatusEnum.PlaceInProgress,
                PicknPlaceStatusEnum.PlaceComplete));

            // Example 2
            // Failure case where grips cannot be constructed, so never reaches pick or place completion
            tasks.Add(Tuple.Create(
                new Pose(new Vector3(-0.9, -0.9614, 0.00)) as Pose?,
                new Pose(new Vector3(-0.650, -0.000, 0.3)) as Pose?,
                (Pose?)null,
                (Pose?)null,
                PicknPlaceStatusEnum.PickFailOutOfRange,
                PicknPlaceStatusEnum.PickFailOutOfRange,
                PicknPlaceStatusEnum.PickFailOutOfRange,
                PicknPlaceStatusEnum.PickFailOutOfRange));

            tasks.Add(Tuple.Create(
                new Pose(new Vector3(-0.351, -0.7614, 0.00)) as Pose?,
                new Pose(new Vector3(-0.650, -0.000, 0.3)) as Pose?,
                new Pose(new Vector3(-0.95, -0.92, 0.000)) as Pose?,
                new Pose(new Vector3(-0.600, -0.000, 0.3)) as Pose?,
                PicknPlaceStatusEnum.PickInProgress,
                PicknPlaceStatusEnum.PickComplete,
                PicknPlaceStatusEnum.PlaceFailOutOfRange,
                PicknPlaceStatusEnum.PlaceFailOutOfRange));

            this.KukaRobotiqAgentTest(tasks);
        }

        /// <summary>
        /// Set up and run the pick and place task for a Kuka / Robotiq
        /// </summary>
        /// <param name="tasks">List of tasks. Each task has four poses- 
        /// pick pose pick retract pose place pose and place retract pose</param>        
        private void KukaRobotiqAgentTest(List<Tuple<Pose?, Pose?, Pose?, Pose?, PicknPlaceStatusEnum, PicknPlaceStatusEnum, PicknPlaceStatusEnum, Tuple<PicknPlaceStatusEnum>>> tasks)
        {
            string armAgentName = "armAgent";
            string handAgentName = "handAgent";
            string pickPlaceAgentName = "pickPlaceAgent";

            FriSimulator testFri = new FriSimulator();
            ArmControlKuka armControl = new ArmControlKuka(testFri);

            // Create a pipeline and send a single Kinect message
            AgentLocator producerLocator = new AgentLocator();

            TimerAgent timer = new TimerAgent("ManipulationHeartbeatTimer", 50, 1);

            KukaLBRControlAgent armAgent = new KukaLBRControlAgent(
                        armAgentName,
                        timer,
                        armControl,
                        0.1,
                        0.1,
                        0.05,
                        false,
                        1);

            MockRobotiqSAgent robotiqAgent = new MockRobotiqSAgent(
                name: handAgentName,
                producer: timer);

            PicknPlaceAgentKukaRobotiq pickPlaceAgent = new PicknPlaceAgentKukaRobotiq(
                                            name: pickPlaceAgentName,
                                            producer: armAgent,
                                            armControlAgentName: armAgentName,
                                            handControlAgentName: handAgentName,
                                            handStatusProvider: handAgentName,
                                            armMoveRotationalTolerance: 0.025 * 5.0,
                                            armMoveTranslationalTolerance: 0.025,
                                            liftHeight: 0.140,
                                            defaultSubTaskStepTimeOutmSec: 1000,
                                            gripExchangePose: new Pose(new Vector3(-0.45, 0.57, 0.005), new Quaternion(0.0, 0.0, 0.7285, -0.6851)),
                                            defaultNeutralPose: new Pose(new Vector3(-0.68, 0.0, 0.15)));

            IAgentHost timerHost = producerLocator.Register(timer);
            IAgentHost manipHost = producerLocator.Register(armAgent);
            IAgentHost gripperHost = producerLocator.Register(robotiqAgent);
            IAgentHost pickPlaceHost = producerLocator.Register(pickPlaceAgent);

            IForwarder<PickAndMoveMessage> pickInjector = pickPlaceHost.GetControlPort<PickAndMoveMessage>();
            IForwarder<PlaceAndMoveMessage> placeInjector = pickPlaceHost.GetControlPort<PlaceAndMoveMessage>();

            MessageCollectorAgent<PicknPlaceStatusMessage> collector = new MessageCollectorAgent<PicknPlaceStatusMessage>("collector", pickPlaceAgent);

            IAgentHost collectorHost = producerLocator.Register(collector);

            double positionTolerance = 0.025;
            double orientTolerance = positionTolerance * 5.0;

            PhysicalObjectDescription obj = new PhysicalObjectDescription(PhysicalObjectShape.Cylinder, 0.075, 0.110, 0.075, 411.0, "canTest");

            try
            {
                pickPlaceHost.Activate();
                gripperHost.Activate();
                Random rand = new Random(100);

                // Wait for FRI control to start
                while (testFri.GetRequestCount <= 1)
                {
                    Thread.Sleep(testFri.CycleTimeinMsec);
                }

                collectorHost.Activate();

                foreach (var task in tasks)
                {
                    Pose? pick = task.Item1;
                    Pose? pickMove = task.Item2;
                    Pose? place = task.Item3;
                    Pose? placeMove = task.Item4;

                    List<AgentMessage> messages = new List<AgentMessage>();

                    if (true == pick.HasValue && true == pickMove.HasValue)
                    {
                        messages.Add(new PickAndMoveMessage(rand.Next(10000), new PickAndMove[] { new PickAndMove(obj, pick.Value, pickMove.Value, PicknPlaceModeEnum.Baseline) }));
                    }

                    if (true == place.HasValue && true == placeMove.HasValue)
                    {
                        messages.Add(new PlaceAndMoveMessage(rand.Next(10000), new PlaceAndMove[] { new PlaceAndMove(obj, place.Value, placeMove.Value) }));
                    }

                    long timeOutTicks = Stopwatch.Frequency * 10;
                    long elapsedTicks = 0;
                    long startTicks = Stopwatch.GetTimestamp();

                    foreach (AgentMessage m in messages)
                    {
                        PicknPlaceStatusEnum terminatingMsg;
                        PicknPlaceStatusEnum requiredExecutionState;

                        if (m is PickAndMoveMessage)
                        {
                            pickInjector.Post(m as PickAndMoveMessage);
                            requiredExecutionState = task.Item5;
                            terminatingMsg = task.Item6;
                        }
                        else if (m is PlaceAndMoveMessage)
                        {
                            placeInjector.Post(m as PlaceAndMoveMessage);
                            requiredExecutionState = task.Item7;
                            terminatingMsg = task.Rest.Item1;
                        }
                        else
                        {
                            throw new Exception("Unkown message type");
                        }

                        collector.Results.Clear();
                        PicknPlaceStatusMessage statusMsg = null;
                        bool cont = true;
                        bool foundRequiredState = false;

                        do
                        {
                            if (null != collector.Results && collector.Results.Count > 0)
                            {
                                statusMsg = collector.Results.Dequeue();
                                
                                if (statusMsg.StatusEnum == requiredExecutionState)
                                {
                                    foundRequiredState = true;
                                }

                                Assert.AreEqual(statusMsg.OriginatingMessageId, m.OriginatingTime);

                                if (statusMsg.StatusEnum == terminatingMsg)
                                {
                                    cont = false;
                                }
                            }
                            else
                            {
                                cont = true;
                            }

                            elapsedTicks = Stopwatch.GetTimestamp() - startTicks;
                            cont &= elapsedTicks <= timeOutTicks;
                        }
                        while (true == cont);
                        Assert.IsTrue(elapsedTicks <= timeOutTicks, "Manipulation Pick and place test timed out");
                        Assert.IsTrue(foundRequiredState, string.Format("Run terminated without encountering a {0} state", requiredExecutionState.ToString()));
                    }
                }
            }
            finally
            {
                collectorHost.Deactivate();
                pickPlaceHost.Deactivate();
                gripperHost.Deactivate();
            }
        }

        /// <summary>
        /// Checks if a move is complete
        /// </summary>
        /// <param name="current">Current TCP pose</param>
        /// <param name="target">Target TCP pose</param>
        /// <param name="orientationTolerance">Orientation tolerance</param>
        /// <param name="positionTolerance">Position tolerance</param>
        /// <returns>True if move is complete</returns>
        private bool IsArmMoveComplete(Pose current, Pose target, double orientationTolerance, double positionTolerance)
        {
            double quatDiff = Quaternion.Length(current.Orientation - target.Orientation);
            double posDiff = current.Position.DistanceFrom(target.Position);
            bool ready = quatDiff < orientationTolerance &&
                posDiff < positionTolerance;

            if (quatDiff > orientationTolerance && Math.Abs(target.Orientation.W) < 0.001)
            {
                // Corner case check for equivalent rotations 
                double equivThreshold = orientationTolerance / 4.0;
                if (Math.Abs(Math.Abs(target.Orientation.X) - Math.Abs(current.Orientation.X)) < equivThreshold &&
                    Math.Abs(Math.Abs(target.Orientation.Y) - Math.Abs(current.Orientation.Y)) < equivThreshold &&
                    Math.Abs(Math.Abs(target.Orientation.Z) - Math.Abs(current.Orientation.Z)) < equivThreshold &&
                    Math.Abs(Math.Abs(target.Orientation.W) - Math.Abs(current.Orientation.W)) < equivThreshold)
                {
                    ready = posDiff < positionTolerance;
                }
            }

            return ready;
        }
    }
}
